//
// Created by 16933 on 2022/3/11.
//

#include "Chassis.h"
#include "cmsis_os.h"
#include "bsp_can.h"
#include "stdbool.h"
#include "usart.h"
extern CAN_HandleTypeDef hcan1;
extern RC_ctrl_t rc_ctrl;
bool flag = true;
extern int16_t current;
extern float speed_set;
void chassis_Task(void const * argument)
{
    for(;;)
    {
        if (rc_ctrl.rc.ch[3]>=500)
        {
            if(rc_ctrl.rc.s[1]==2)
            {
                if (flag)
                {
                    speed_set+=200;
                }
                else
                {
                    speed_set-=200;
                }
                HAL_GPIO_WritePin(GPIOE,GPIO_PIN_7,RESET);
                osDelay(30);
                HAL_GPIO_WritePin(GPIOE,GPIO_PIN_7,SET);
            }

        }
        else if(rc_ctrl.rc.ch[3]<=-500)
        {
            if(rc_ctrl.rc.s[1]==2)
            {
                if (flag)
                {
                    speed_set-=200;
                }
                else
                {
                    speed_set+=200;
                }
                HAL_GPIO_WritePin(GPIOF,GPIO_PIN_14,RESET);
                osDelay(30);
                HAL_GPIO_WritePin(GPIOF,GPIO_PIN_14,SET);
            }

        }
        if (rc_ctrl.rc.s[1]==2)
        {

            if (flag)
            {
                CAN_Sendcurrnt(&hcan1, 2, current);
            }
            else
            {
                CAN_Sendcurrnt(&hcan1, 2, current);
            }

        }
        else
        {
            speed_set = 0;
            CAN_Sendcurrnt(&hcan1, 2, current);
        }
        osDelay(1);
    }

}

